#pragma once
#include <memory>
#include <vector>
#include "pfsm_state.h"

#include "simple_tp.h"
#include "zuc/nml_intf/zucpos2.h"

namespace axisgroup {
class AxisGroup;
class PFsmFree : public PFsmState
{
public:
    static const uint32_t JOG_CONT_COUNTDOWN = 125;

public:
    enum JogType
    {
        JOG_JOINT = 0,
        JOG_CART_WORLD = 1,
        JOG_CART_TOOL = 2,
    } jog_type_;
    PFsmFree(Robot* grp);
    virtual int switch_jog_type(JogType type);
    void reset_jog_countdown() { jog_cont_countdown_ = JOG_CONT_COUNTDOWN; };

private:
    int update_joint_jog(RobJointVal& jcmd, ZucPose& pcmd);
    int update_cart_jog(RobJointVal& jcmd, ZucPose& pcmd);
    int check_jog_max_vel(simple_tp_t* tp, int jog_num);
    int calc_jog_length(int active_num, double* length);
    int pre_check_safezone_JOG_TCP(simple_tp_t* tp, ZucPose* toolCoordOffset, int axis_num);
    bool check_inpos();

    virtual int update_plan() override;
    virtual int enter() override;
    virtual void exit() override { return; }
    void abort() override;
    virtual bool is_stopping() override;
    virtual bool is_plan_over() override;

    bool check_jog_cont_countdown();
    uint32_t jog_cont_countdown_ = 0;  // jog continue 指令计数
    ZucPose tool_coord_offset_ = {};   // 该变量用于记录进入free模式时的姿态，姿态jog时将基于此进行

public:
    JogType get_free_type() const { return jog_type_; }
    std::vector<simple_tp_t> free_tp_;
    std::vector<simple_tp_t> free_tp_cart_;
    bool is_in_incr = false;  // 上一个、或当前期望是INCR模式
};
}  // namespace axisgroup
